﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using NIP.DynamicControl.Environ;
using NIP.ControllerInterface;

namespace NIP.DynamicControl.Constants
{

    public class PIDconst
    {

        public const double J0kp = 0.0192;
        public const double J0ki = 0.0;
        public const double J0kd = 0.0044;

        public const double J1kp = 0.025;
        public const double J1ki = 0.00014;
        public const double J1kd = 0.0063;

        public const double J2kp = 0.0183;
        public const double J2ki = 0.00012;
        public const double J2kd = 0.003;

        public const double J3kp = 0.015;
        public const double J3ki = 0.0015;
        public const double J3kd = 0.0035;

        public const double J4kp = 0.0095;
        public const double J4ki = 0.0;
        public const double J4kd = 0.002;
    }

    
    public class gridconst
    {
       
        public enum Angles
        {
            Theta1 = 0,
            Theta2,
            Theta3,
            Theta4,
            Theta5,
            Theta6,
            Min = 0,
            Max,
            Deg = 0,
            Rad,
        };
    
        // grid and workspace constants
        public const double mmScale = 200; // 200 units = 1 mm
        public const double xmin = -900; 
        public const double xmax = 900;
        public const double ymin = -900;
        public const double ymax = 900;
        public const double zmin = 0;
        public const double zmax = 1200;
        public const double GRIDRES = 80;
        public const double convergence = 0.00001;
        public const double goalForce = -100;
        public const double pathVel = 150.0; // in mm/s
        public const double planningDelay = 0.5; // secs

        public const double imgcenterX = -110.0;
        public const double imgcenterY = 280.0;
        //public const double imgcenterZ = 170.0;
        public const double imgcenterZ = 0.0;
        public const double DrawContourZOffset = 50.0;
        


        public const double theta1min = -144;
        public const double theta1max = 144;
        //public const double theta1fudge = ((10.6 * Math.PI) / 180.0); //fudge factor; scorbot slightly misaligned with field
        public const double theta1fudge = 10.6;
        public const double theta2min = -100; //was -44
        public const double theta2max = 43; //was 101
        public const double theta3min = -104;
        public const double theta3max = 104;
        public const double theta4min = -95;
        public const double theta4max = 95;
        public const double theta5min = -270;
        public const double theta5max = 270;
        public const double minZcoord = 0.0;

        public static int xcellspos = (int)Math.Round((Math.Abs(xmax) / GRIDRES), 0, MidpointRounding.AwayFromZero);
        public static int xcellsneg = (int)Math.Round((Math.Abs(xmin) / GRIDRES), 0, MidpointRounding.AwayFromZero);
        public static int ycellspos = (int)Math.Round((Math.Abs(ymax) / GRIDRES), 0, MidpointRounding.AwayFromZero);
        public static int ycellsneg = (int)Math.Round((Math.Abs(ymin) / GRIDRES), 0, MidpointRounding.AwayFromZero);
        public static int zcellspos = (int)Math.Round((Math.Abs(zmax) / GRIDRES), 0, MidpointRounding.AwayFromZero);
        public static int zcellsneg = (int)Math.Round((Math.Abs(zmin) / GRIDRES), 0, MidpointRounding.AwayFromZero);
        public static int xcells = xcellspos + xcellsneg + 2;
        public static int ycells = ycellspos + ycellsneg + 2;
        public static int zcells = zcellspos + zcellsneg + 2;
               
        
        //public static int x0cell = gridZeroCell(xmin);
        //public static int y0cell = gridZeroCell(ymin);
        //public static int z0cell = gridZeroCell(zmin);

        public const double Baseradius = 200.0;
        public const double Baseheight = 600.0;
        public const double L0 = 70;
        public const double L1 = 388;
        public const double L2 = 280;
        public const double L3 = 230;
        public const double L4 = 42;
        
        public const double D5gripOffset = 121;
        //! Quick fix to take into account the brush -- was imgCentreZ
        public const double D5grip = 231 + 170;
        public const double D6grip = 0;
        public const double D5wrist = 0;
        public const double D6wrist = 0;
        public const double D5cam = 80;
        public const double D5camMinOffset = 150;
        public const double D6cam = 60;
        public const double D5point = D5grip;
        public const double D5pointMinOffset = 50;
        public const double D6point = 0;
        public const double L23max = L2 + L3;
        
        //public const double L23min = Math.Sqrt((L2 * L2) + (L3 * L3) - (2.0 * L2 * L3 * Math.Acos(theta3min)));

                
        public static int[] XYZtoGrid(double[] xyz)
        {
            int[] XYZreturn = new int[USBC.TotalAxes];
            XYZreturn[0] = (int)((xyz[0] - xmin) / GRIDRES) + 1;
            XYZreturn[1] = (int)((xyz[1] - ymin) / GRIDRES) + 1;
            XYZreturn[2] = (int)((xyz[2] - zmin) / GRIDRES) + 1;
#if DEBUG
            Debug.WriteLine("XYZtoGrid(double) [" + (int)xyz[0] + "," + (int)xyz[1] + "," + (int)xyz[2] + "] = [" + XYZreturn[0] + "," + XYZreturn[1] + "," + XYZreturn[2] + "]");
#endif
            
            return XYZreturn;
        }
       
        public static int[] XYZtoGrid(int[] xyz)
        {
            int[] XYZreturn = new int[USBC.TotalAxes];
            XYZreturn[0] = (int)((xyz[0] - xmin) / GRIDRES) + 1;
            XYZreturn[1] = (int)((xyz[1] - ymin) / GRIDRES) + 1;
            XYZreturn[2] = (int)((xyz[2] - zmin) / GRIDRES) + 1;
#if DEBUG
            Debug.WriteLine("XYZtoGrid(int) [" + xyz[0] + "," + xyz[1] + "," + xyz[2] + "] = [" + XYZreturn[0] + "," + XYZreturn[1] + "," + XYZreturn[2] + "]");
#endif

            return XYZreturn;
        }

        public static int[] XYZtoGrid(PathPoint position)
        {
            int[] XYZreturn = new int[USBC.TotalAxes];
            XYZreturn[0] = (int)(((position.pos[0]) - xmin) / GRIDRES) + 1;
            XYZreturn[1] = (int)(((position.pos[1]) - ymin) / GRIDRES) + 1;
            XYZreturn[2] = (int)(((position.pos[2]) - zmin) / GRIDRES) + 1;
#if DEBUG
            //Debug.WriteLine("XYZtoGrid(Position) [" + position.XYZd[0] + "," + position.XYZd[1] + "," + position.XYZd[2] + "] = [" + XYZreturn[0] + "," + XYZreturn[1] + "," + XYZreturn[2] + "]");
#endif

            return XYZreturn;
        }
                

        public static double toRadians(double angle)
        {
            return ((angle * Math.PI) / 180.0);
        }
        public static double toDegrees(double angle)
        {
            return ((angle * 180.0) / Math.PI);
        }
        

    }
}
